#include "astarlocalpath.h"
#include <math.h>
#include <memory>
class MapSearchNode;
class NavmeshRegion;

class GridSearch:public AStarAbstract{
private:
	shared_ptr<Point>end;
	NavmeshRegion*region;
public:
	GridSearch(NavmeshRegion*region, shared_ptr<Point>end){
		this->region=region;
		this->end=end;
	}
	
    bool isGoal(shared_ptr<Point>node) {
		return (node->getX()==end->getX()) && (node->getY()==end->getY());
    }

    
    float g(shared_ptr<Point>from, shared_ptr<Point>to) {
		if (from->getX() == to->getX() && from->getY()==to->getY()) {
            return 0.0;
        }

        return squaredLength(from, to);
    }

    
    float h(shared_ptr<Point>from, shared_ptr<Point>to) {
		return (float)(abs((double)from->getX() - to->getX()) + abs((double)from->getY() - to->getY()));
    }

    float squaredLength(shared_ptr<Point>p1, shared_ptr<Point>p2) {
		double a = p1->getX() - p2->getX();
        a = a * a;
		double b = p1->getY() - p2->getY();
        b = b * b;
        return (float)(a + b);
    }

	bool isCellSuccessor(int x, int y){
		if(x<0 || y<0 ||x>=width || y>=height){
			return false;
		}

		return cells[y][x];
	}

	void generateSuccessors(shared_ptr<Point>node, std::vector<shared_ptr<Point>>*successorsOutputList) {
		int x;
		int y;

		x=node->getX();
		y=node->getY()-1;
		if(isCellSuccessor(x,y)){
			successorsOutputList->push_back(shared_ptr<Point>(new Point(x,y)));
		}

		x=node->getX()-1;
		y=node->getY();
		if(isCellSuccessor(x,y)){
			successorsOutputList->push_back(shared_ptr<Point>(new Point(x,y)));
		}

		x=node->getX()+1;
		y=node->getY();
		if(isCellSuccessor(x,y)){
			successorsOutputList->push_back(shared_ptr<Point>(new Point(x,y)));
		}

		x=node->getX();
		y=node->getY()+1;
		if(isCellSuccessor(x,y)){
			successorsOutputList->push_back(shared_ptr<Point>(new Point(x,y)));
		}
    }


    float squaredLengthToGoal(shared_ptr<Point>cell) {
		double a = end->getX() - cell->getX();
        a = a * a;
		double b = end->getY() - cell->getY();
        b = b * b;
        return (float)(a + b);
    }
};